#include "ssd1306.hpp"
#include "spi_driver.hpp"
// #include "i2c_interface.hpp"
#include "esp_log.h"
#include <cstring>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "font8x8_basic.h"

#define TAG "SSD1306"
#define SPI_COMMAND_MODE 0
#define SPI_DATA_MODE 1
#define SPI_DEFAULT_FREQUENCY 10000000 // 1MHz
// ...existing code...

SSD1306::SSD1306()
{
}

SSD1306::SSD1306(int width, int height)
    : _width(width), _height(height), _pages(height / 8), _scEnable(false), _flip(false)
{
    // Initialize internal buffer
    for (int i = 0; i < _pages; i++) {
        memset(_page[i]._segs, 0, 128);
    }
}

SSD1306::~SSD1306()
{
    // Destructor implementation
}

void SSD1306::init(int mosi, int sclk, spi_host_device_t spi_host, int cs, int dc, int reset)
{
    spiDriver.init(mosi, -1, sclk, spi_host);
    spiDriver.setClockSpeed(SPI_DEFAULT_FREQUENCY);
    spiDriver.addDevice(cs);
    _dc = dc;
    _address = SPI_ADDRESS;
    // SPI Initialization code
    _spiHWInit();
}

void SSD1306::init(SSD1306DriverConfig *config)
{
	memcpy(&i2cDriver, config, sizeof(SSD1306DriverConfig));
	if (this->mI2CDev == NULL) {
        i2c_master_bus_add_device(this->i2cDriver.busHandle, &this->i2cDriver.i2cConfig, &this->mI2CDev);
    }

    if (!this->mI2CDev) {
        ESP_LOGE(TAG, "Failed to add I2C device");
    }
    _address = I2C_ADDRESS;
    // I2C Initialization code
    _i2cHWInit();
}

int SSD1306::getWidth() const
{
    return _width;
}

int SSD1306::getHeight() const
{
    return _height;
}

int SSD1306::getPages() const
{
    return _pages;
}

void SSD1306::showBuffer()
{
    if (_address == SPI_ADDRESS) {
        for (int page = 0; page < _pages; page++) {
            spiDriver.writeByte(_page[page]._segs, _width);
        }
    } else {
        for (int page = 0; page < _pages; page++) {
            // i2cInterface.write(_address, _page[page]._segs, _width);
			i2c_master_transmit(mI2CDev, _page[page]._segs, _width, -1);
        }
    }
}

void SSD1306::setBuffer(uint8_t *buffer)
{
    int index = 0;
    for (int page = 0; page < _pages; page++) {
        memcpy(&_page[page]._segs, &buffer[index], 128);
        index += 128;
    }
}

void SSD1306::getBuffer(uint8_t *buffer)
{
    int index = 0;
    for (int page = 0; page < _pages; page++) {
        memcpy(&buffer[index], &_page[page]._segs, 128);
        index += 128;
    }
}

void SSD1306::setPage(int page, uint8_t *buffer)
{
    memcpy(&_page[page]._segs, buffer, 128);
}

void SSD1306::getPage(int page, uint8_t *buffer)
{
    memcpy(buffer, &_page[page]._segs, 128);
}

void SSD1306::displayImage(int page, int seg, uint8_t *images, int width)
{
    if (_address ==	SPI_ADDRESS) {
        spiDriver.writeByte(images, width);
    } else {
        // i2cInterface.write(_address, images, width);
		i2c_master_transmit(mI2CDev, images, width, -1);
    }
    // Set to internal buffer
    memcpy(&_page[page]._segs[seg], images, width);
}

void SSD1306::displayText(int page, const char *text, int text_len, bool _invert)
{
    if (page >= _pages) return;
    int _text_len = text_len;
    if (_text_len > 16) _text_len = 16;

    int seg = 0;
    uint8_t image[8];
    for (int i = 0; i < _text_len; i++) {
        memcpy(image, font8x8_basic_tr[(uint8_t)text[i]], 8);
        if (_invert) invert(image, 8);
        if (_flip) flip(image, 8);
        displayImage(page, seg, image, 8);
        seg += 8;
    }
}

void SSD1306::clearScreen(bool invert)
{
    char space[16];
    memset(space, 0x00, sizeof(space));
    for (int page = 0; page < _pages; page++) {
        displayText(page, space, sizeof(space), invert);
    }
}

void SSD1306::clearLine(int page, bool invert)
{
    char space[16];
    memset(space, 0x00, sizeof(space));
    displayText(page, space, sizeof(space), invert);
}

void SSD1306::setContrast(int contrast)
{
    if (_address == SPI_ADDRESS) {
		spiContrast(contrast);
	} else {
		i2cContrast(contrast);
	}
}

void SSD1306::softwareScroll(int start, int end)
{
    // Software scroll implementation
    ESP_LOGD(__FUNCTION__, "software_scroll start=%d end=%d _pages=%d", start, end, _pages);
	if (start < 0 || end < 0) {
		_scEnable = false;
	} else if (start >= _pages || end >= _pages) {
		_scEnable = false;
	} else {
		_scEnable = true;
		_scStart = start;
		_scEnd = end;
		_scDirection = 1;
		if (start > end ) _scDirection = -1;
	}
}

void SSD1306::scrollText(const char *text, int text_len, bool invert)
{
    // Scroll text implementation
    ESP_LOGD(__FUNCTION__, "_scEnable=%d", _scEnable);
	if (_scEnable == false) return;

	int srcIndex = _scEnd - _scDirection;
	while(1) {
		int dstIndex = srcIndex + _scDirection;
		ESP_LOGD(__FUNCTION__, "srcIndex=%d dstIndex=%d", srcIndex,dstIndex);
		for(int seg = 0; seg < _width; seg++) {
			_page[dstIndex]._segs[seg] = _page[srcIndex]._segs[seg];
		}
		if (_address == SPI_ADDRESS) {
			this->spiDisplayImage(dstIndex, 0, _page[dstIndex]._segs, sizeof(_page[dstIndex]._segs));
		} else {
			this->i2cDisplayImage(dstIndex, 0, _page[dstIndex]._segs, sizeof(_page[dstIndex]._segs));
		}
		if (srcIndex == _scStart) break;
		srcIndex = srcIndex - _scDirection;
	}
	
	int _text_len = text_len;
	if (_text_len > 16) _text_len = 16;
	
	displayText(srcIndex, text, text_len, invert);
}

void SSD1306::scrollClear()
{
    ESP_LOGD(__FUNCTION__, "_scEnable=%d", _scEnable);
	if (_scEnable == false) return;

	int srcIndex = _scEnd - _scDirection;
	while(1) {
		int dstIndex = srcIndex + _scDirection;
		ESP_LOGD(__FUNCTION__, "srcIndex=%d dstIndex=%d", srcIndex,dstIndex);
		clearLine(dstIndex, false);
		if (dstIndex == _scStart) break;
		srcIndex = srcIndex - _scDirection;
	}
}

void SSD1306::hardwareScroll(ScrollType scroll)
{
    // Hardware scroll implementation
    if (_address == SPI_ADDRESS) {
		spiHardwareScroll(scroll);
	} else {
		i2cHardwareScroll(scroll);
	}
}

void SSD1306::wrapAround(ScrollType scroll, int start, int end, int8_t delay)
{
    // Wrap around implementation
    if (scroll == SCROLL_RIGHT) {
		int _start = start; // 0 to 7
		int _end = end; // 0 to 7
		if (_end >= _pages) _end = _pages - 1;
		uint8_t wk;
		//for (int page=0;page<_pages;page++) {
		for (int page=_start;page<=_end;page++) {
			wk = _page[page]._segs[127];
			for (int seg=127;seg>0;seg--) {
				_page[page]._segs[seg] = _page[page]._segs[seg-1];
			}
			_page[page]._segs[0] = wk;
		}

	} else if (scroll == SCROLL_LEFT) {
		int _start = start; // 0 to 7
		int _end = end; // 0 to 7
		if (_end >= _pages) _end = _pages - 1;
		uint8_t wk;
		//for (int page=0;page<_pages;page++) {
		for (int page=_start;page<=_end;page++) {
			wk = _page[page]._segs[0];
			for (int seg=0;seg<127;seg++) {
				_page[page]._segs[seg] = _page[page]._segs[seg+1];
			}
			_page[page]._segs[127] = wk;
		}

	} else if (scroll == SCROLL_UP) {
		int _start = start; // 0 to {width-1}
		int _end = end; // 0 to {width-1}
		if (_end >= _width) _end = _width - 1;
		uint8_t wk0;
		uint8_t wk1;
		uint8_t wk2;
		uint8_t save[128];
		// Save pages 0
		for (int seg=0;seg<128;seg++) {
			save[seg] = _page[0]._segs[seg];
		}
		// Page0 to Page6
		for (int page=0;page<_pages-1;page++) {
			//for (int seg=0;seg<128;seg++) {
			for (int seg=_start;seg<=_end;seg++) {
				wk0 = _page[page]._segs[seg];
				wk1 = _page[page+1]._segs[seg];
				if (_flip) wk0 = rotateByte(wk0);
				if (_flip) wk1 = rotateByte(wk1);
				if (seg == 0) {
					ESP_LOGD(__FUNCTION__, "b page=%d wk0=%02x wk1=%02x", page, wk0, wk1);
				}
				wk0 = wk0 >> 1;
				wk1 = wk1 & 0x01;
				wk1 = wk1 << 7;
				wk2 = wk0 | wk1;
				if (seg == 0) {
					ESP_LOGD(__FUNCTION__, "a page=%d wk0=%02x wk1=%02x wk2=%02x", page, wk0, wk1, wk2);
				}
				if (_flip) wk2 = rotateByte(wk2);
				_page[page]._segs[seg] = wk2;
			}
		}
		// Page7
		int pages = _pages-1;
		//for (int seg=0;seg<128;seg++) {
		for (int seg=_start;seg<=_end;seg++) {
			wk0 = _page[pages]._segs[seg];
			wk1 = save[seg];
			if (_flip) wk0 = rotateByte(wk0);
			if (_flip) wk1 = rotateByte(wk1);
			wk0 = wk0 >> 1;
			wk1 = wk1 & 0x01;
			wk1 = wk1 << 7;
			wk2 = wk0 | wk1;
			if (_flip) wk2 = rotateByte(wk2);
			_page[pages]._segs[seg] = wk2;
		}

	} else if (scroll == SCROLL_DOWN) {
		int _start = start; // 0 to {width-1}
		int _end = end; // 0 to {width-1}
		if (_end >= _width) _end = _width - 1;
		uint8_t wk0;
		uint8_t wk1;
		uint8_t wk2;
		uint8_t save[128];
		// Save pages 7
		int pages = _pages-1;
		for (int seg=0;seg<128;seg++) {
			save[seg] = _page[pages]._segs[seg];
		}
		// Page7 to Page1
		for (int page=pages;page>0;page--) {
			//for (int seg=0;seg<128;seg++) {
			for (int seg=_start;seg<=_end;seg++) {
				wk0 = _page[page]._segs[seg];
				wk1 = _page[page-1]._segs[seg];
				if (_flip) wk0 = rotateByte(wk0);
				if (_flip) wk1 = rotateByte(wk1);
				if (seg == 0) {
					ESP_LOGD(__FUNCTION__, "b page=%d wk0=%02x wk1=%02x", page, wk0, wk1);
				}
				wk0 = wk0 << 1;
				wk1 = wk1 & 0x80;
				wk1 = wk1 >> 7;
				wk2 = wk0 | wk1;
				if (seg == 0) {
					ESP_LOGD(__FUNCTION__, "a page=%d wk0=%02x wk1=%02x wk2=%02x", page, wk0, wk1, wk2);
				}
				if (_flip) wk2 = rotateByte(wk2);
				_page[page]._segs[seg] = wk2;
			}
		}
		// Page0
		//for (int seg=0;seg<128;seg++) {
		for (int seg=_start;seg<=_end;seg++) {
			wk0 = _page[0]._segs[seg];
			wk1 = save[seg];
			if (_flip) wk0 = rotateByte(wk0);
			if (_flip) wk1 = rotateByte(wk1);
			wk0 = wk0 << 1;
			wk1 = wk1 & 0x80;
			wk1 = wk1 >> 7;
			wk2 = wk0 | wk1;
			if (_flip) wk2 = rotateByte(wk2);
			_page[0]._segs[seg] = wk2;
		}

	} else if (scroll == PAGE_SCROLL_DOWN) {
		uint8_t save[128];
		// Save pages 7
		for (int seg=0;seg<128;seg++) {
			save[seg] = _page[_pages-1]._segs[seg];
		}
		// Page7 to Page1
		for (int page=_pages-1;page>0;page--) {
			for (int seg=0;seg<128;seg++) {
				_page[page]._segs[seg] = _page[page-1]._segs[seg];
			}
		}
		// Store  pages 0
		for (int seg=0;seg<128;seg++) {
			_page[0]._segs[seg] = save[seg];
		}

	} else if (scroll == PAGE_SCROLL_UP) {
		uint8_t save[128];
		// Save pages 0
		for (int seg=0;seg<128;seg++) {
			save[seg] = _page[0]._segs[seg];
		}
		// Page0 to Page6
		for (int page=0;page<_pages-1;page++) {
			for (int seg=0;seg<128;seg++) {
				_page[page]._segs[seg] = _page[page+1]._segs[seg];
			}
		}
		// Store  pages 7
		for (int seg=0;seg<128;seg++) {
			_page[_pages-1]._segs[seg] = save[seg];
		}
	}

	if (delay >= 0) {
		for (int page=0;page<_pages;page++) {
			if (_address == SPI_ADDRESS) {
				spiDisplayImage(page, 0, _page[page]._segs, 128);
			} else {
				i2cDisplayImage(page, 0, _page[page]._segs, 128);
			}
			if (delay) vTaskDelay(delay);
		}
	}
}

void SSD1306::bitmaps(int xpos, int ypos, uint8_t *bitmap, int width, int height, bool invert)
{
    // Bitmaps implementation
    if ( (width % 8) != 0) {
		ESP_LOGE(__FUNCTION__, "width must be a multiple of 8");
		return;
	}
	int _width = width / 8;
	uint8_t wk0;
	uint8_t wk1;
	uint8_t wk2;
	uint8_t page = (ypos / 8);
	uint8_t _seg = xpos;
	uint8_t dstBits = (ypos % 8);
	ESP_LOGD(__FUNCTION__, "_width=%d ypos=%d page=%d dstBits=%d", _width, ypos, page, dstBits);
	int offset = 0;
	for(int _height=0;_height<height;_height++) {
		for (int index=0;index<_width;index++) {
			for (int srcBits=7; srcBits>=0; srcBits--) {
				wk0 = _page[page]._segs[_seg];
				if (_flip) wk0 = rotateByte(wk0);

				wk1 = bitmap[index+offset];
				if (invert) wk1 = ~wk1;

				//wk2 = ssd1306_copy_bit(bitmap[index+offset], srcBits, wk0, dstBits);
				wk2 = copyBit(wk1, srcBits, wk0, dstBits);
				if (_flip) wk2 = rotateByte(wk2);

				ESP_LOGD(__FUNCTION__, "index=%d offset=%d wk1=0x%x page=%d _seg=%d, wk2=%02x", index, offset, wk1, page, _seg, wk2);
				if (_seg >= 128) {
					ESP_LOGW(__FUNCTION__, "segment is out of range");
					break;
				}
				if (page >= _pages) {
					ESP_LOGW(__FUNCTION__, "page is out of range");
					break;
				}
				_page[page]._segs[_seg] = wk2;
				_seg++;
			}
		}
		vTaskDelay(1);
		offset = offset + _width;
		dstBits++;
		_seg = xpos;
		if (dstBits == 8) {
			page++;
			dstBits=0;
		}
	}
    showBuffer();
}

void SSD1306::fadeOut()
{
    // Fade out implementation
    uint8_t image[1];
    for(int page=0; page<_pages; page++) {
        image[0] = 0xFF;
        for(int line=0; line<8; line++) {
            if (_flip) {
                image[0] = image[0] >> 1;
            } else {
                image[0] = image[0] << 1;
            }
            for(int seg=0; seg<128; seg++) {
                if (_address == SPI_ADDRESS) {
                    spiDisplayImage(page, seg, image, 1);
                } else {
                    i2cDisplayImage(page, seg, image, 1);
                }
                _page[page]._segs[seg] = image[0];
            }
        }
    }
}

void SSD1306::rotateImage(uint8_t *image, bool _flip)
{
    // Rotate image implementation
    uint8_t _image[8];
	uint8_t _smask = 0x01;
	for (int i=0;i<8;i++) {
		uint8_t _dmask = 0x80;
		_image[i] = 0;
		for (int j=0;j<8;j++) {
			uint8_t _wk = image[j] & _smask;
			ESP_LOGD(__FUNCTION__, "image[%d]=0x%x _smask=0x%x _wk=0x%x", j, image[j], _smask, _wk);
			if (_wk != 0) {
				_image[i] = _image[i] + _dmask;
			}
			_dmask = _dmask >> 1;
		}
		_smask = _smask << 1;
	}

	for (int i=0;i<8;i++) {
		image[i] = _image[i];
	}
	if (_flip) flip(image, 8);
}

void SSD1306::displayRotateText(int seg, const char *text, int text_len, bool _invert)
{
    int _text_len = text_len;
    if (_text_len > 8) _text_len = 8;
    uint8_t image[8];
    int _page = _pages - 1;
    for (uint8_t i = 0; i < _text_len; i++) {
        memcpy(image, font8x8_basic_tr[(uint8_t)text[i]], 8);
        rotateImage(image, _flip);
        if (_invert) invert(image, 8);
        displayImage(_page, seg, image, 8);
        _page--;
        if (_page < 0) return;
    }
}

void SSD1306::dump() const
{
    ESP_LOGI(TAG, "_address=%x\n", _address);
    ESP_LOGI(TAG, "_width=%x\n", _width);
    ESP_LOGI(TAG, "_height=%x\n", _height);
    ESP_LOGI(TAG, "_pages=%x\n", _pages);
}

void SSD1306::dumpPage(int page, int seg) const
{
    ESP_LOGI(TAG, "page[%d].segs[%d]=%02x", page, seg, _page[page]._segs[seg]);
}

void SSD1306::hardwareInit()
{
    if (_address == SPI_ADDRESS) {
        _spiHWInit();
    } else {
        _i2cHWInit();
    }
}

void SSD1306::_spiHWInit()
{
	_pages = 8;
	if (_height == 32) _pages = 4;

	writeCommand(OLED_CMD_DISPLAY_OFF);			// AE
	writeCommand(OLED_CMD_SET_MUX_RATIO);			// A8
	if (_height == 64) writeCommand(0x3F);
	if (_height == 32) writeCommand(0x1F);
	writeCommand(OLED_CMD_SET_DISPLAY_OFFSET);		// D3
	writeCommand(0x00);
	writeCommand(OLED_CONTROL_BYTE_DATA_STREAM);	// 40
	if (_flip) {
		writeCommand(OLED_CMD_SET_SEGMENT_REMAP_0);	// A0
	} else {
		writeCommand(OLED_CMD_SET_SEGMENT_REMAP_1);	// A1
	}
	//writeCommand(OLED_CMD_SET_SEGMENT_REMAP);		// A1
	writeCommand(OLED_CMD_SET_COM_SCAN_MODE);		// C8
	writeCommand(OLED_CMD_SET_DISPLAY_CLK_DIV);	// D5
	writeCommand(0x80);
	writeCommand(OLED_CMD_SET_COM_PIN_MAP);		// DA
	if (_height == 64) writeCommand(0x12);
	if (_height == 32) writeCommand(0x02);
	writeCommand(OLED_CMD_SET_CONTRAST);			// 81
	writeCommand(0xFF);
	writeCommand(OLED_CMD_DISPLAY_RAM);			// A4
	writeCommand(OLED_CMD_SET_VCOMH_DESELCT);		// DB
	writeCommand(0x40);
	writeCommand(OLED_CMD_SET_MEMORY_ADDR_MODE);	// 20
	//writeCommand(OLED_CMD_SET_HORI_ADDR_MODE);	// 00
	writeCommand(OLED_CMD_SET_PAGE_ADDR_MODE);		// 02
	// Set Lower Column Start Address for Page Addressing Mode
	writeCommand(0x00);
	// Set Higher Column Start Address for Page Addressing Mode
	writeCommand(0x10);
	writeCommand(OLED_CMD_SET_CHARGE_PUMP);		// 8D
	writeCommand(0x14);
	writeCommand(OLED_CMD_DEACTIVE_SCROLL);		// 2E
	writeCommand(OLED_CMD_DISPLAY_NORMAL);			// A6
	writeCommand(OLED_CMD_DISPLAY_ON);				// AF
}

void SSD1306::_i2cHWInit()
{
    _pages = 8;
    if (_height == 32) _pages = 4;

    uint8_t out_buf[27];
    int out_index = 0;
    out_buf[out_index++] = OLED_CONTROL_BYTE_CMD_STREAM;
    out_buf[out_index++] = OLED_CMD_DISPLAY_OFF;				// AE
    out_buf[out_index++] = OLED_CMD_SET_MUX_RATIO;			 // A8
    if (_height == 64) out_buf[out_index++] = 0x3F;
    if (_height == 32) out_buf[out_index++] = 0x1F;
    out_buf[out_index++] = OLED_CMD_SET_DISPLAY_OFFSET;		 // D3
    out_buf[out_index++] = 0x00;
    //out_buf[out_index++] = OLED_CONTROL_BYTE_DATA_STREAM;	// 40
    out_buf[out_index++] = OLED_CMD_SET_DISPLAY_START_LINE;	// 40
    //out_buf[out_index++] = OLED_CMD_SET_SEGMENT_REMAP;		// A1
    if (_flip) {
        out_buf[out_index++] = OLED_CMD_SET_SEGMENT_REMAP_0; // A0
    } else {
        out_buf[out_index++] = OLED_CMD_SET_SEGMENT_REMAP_1;	// A1
    }
    out_buf[out_index++] = OLED_CMD_SET_COM_SCAN_MODE;		// C8
    out_buf[out_index++] = OLED_CMD_SET_DISPLAY_CLK_DIV;		// D5
    out_buf[out_index++] = 0x80;
    out_buf[out_index++] = OLED_CMD_SET_COM_PIN_MAP;			// DA
    if (_height == 64) out_buf[out_index++] = 0x12;
    if (_height == 32) out_buf[out_index++] = 0x02;
    out_buf[out_index++] = OLED_CMD_SET_CONTRAST;			// 81
    out_buf[out_index++] = 0xFF;
    out_buf[out_index++] = OLED_CMD_DISPLAY_RAM;				// A4
    out_buf[out_index++] = OLED_CMD_SET_VCOMH_DESELCT;		// DB
    out_buf[out_index++] = 0x40;
    out_buf[out_index++] = OLED_CMD_SET_MEMORY_ADDR_MODE;	// 20
    //out_buf[out_index++] = OLED_CMD_SET_HORI_ADDR_MODE;	// 00
    out_buf[out_index++] = OLED_CMD_SET_PAGE_ADDR_MODE;		// 02
    // Set Lower Column Start Address for Page Addressing Mode
    out_buf[out_index++] = 0x00;
    // Set Higher Column Start Address for Page Addressing Mode
    out_buf[out_index++] = 0x10;
    out_buf[out_index++] = OLED_CMD_SET_CHARGE_PUMP;			// 8D
    out_buf[out_index++] = 0x14;
    out_buf[out_index++] = OLED_CMD_DEACTIVE_SCROLL;			// 2E
    out_buf[out_index++] = OLED_CMD_DISPLAY_NORMAL;			// A6
    out_buf[out_index++] = OLED_CMD_DISPLAY_ON;				// AF

	
    esp_err_t res;
    res = i2c_master_transmit(mI2CDev, out_buf, out_index, -1);
    if (res == ESP_OK) {
        ESP_LOGI(TAG, "OLED configured successfully");
    } else {
        ESP_LOGE(TAG, "Could not write to device [0x%02x]: %d (%s)", _address, res, esp_err_to_name(res));
    }
}

void SSD1306::writeCommand(uint8_t cmd)
{
    uint8_t wb_data[1] = { cmd };
    gpio_set_level((gpio_num_t)_dc, SPI_COMMAND_MODE);
    spiDriver.writeByte(wb_data, 1);
}

void SSD1306::writeData(uint8_t *data, size_t len)
{
    gpio_set_level((gpio_num_t)_dc, SPI_DATA_MODE);
    spiDriver.writeByte(data, 1);
}

// Private methods implementation
void SSD1306::pixel(int xpos, int ypos, bool invert)
{
    uint8_t __page = (ypos / 8);
    uint8_t _bits = (ypos % 8);
    uint8_t _seg = xpos;
    uint8_t wk0 = _page[__page]._segs[_seg];
    uint8_t wk1 = 1 << _bits;
    if (invert) {
        wk0 = wk0 & ~wk1;
    } else {
        wk0 = wk0 | wk1;
    }
    if (_flip) wk0 = rotateByte(wk0);
    _page[__page]._segs[_seg] = wk0;
}

void SSD1306::line(int x1, int y1, int x2, int y2, bool invert)
{
    int i;
    int dx, dy;
    int sx, sy;
    int E;

    dx = (x2 > x1) ? x2 - x1 : x1 - x2;
    dy = (y2 > y1) ? y2 - y1 : y1 - y2;

    sx = (x2 > x1) ? 1 : -1;
    sy = (y2 > y1) ? 1 : -1;

    if (dx > dy) {
        E = -dx;
        for (i = 0; i <= dx; i++) {
            pixel(x1, y1, invert);
            x1 += sx;
            E += 2 * dy;
            if (E >= 0) {
                y1 += sy;
                E -= 2 * dx;
            }
        }
    } else {
        E = -dy;
        for (i = 0; i <= dy; i++) {
            pixel(x1, y1, invert);
            y1 += sy;
            E += 2 * dx;
            if (E >= 0) {
                x1 += sx;
                E -= 2 * dy;
            }
        }
    }
}

void SSD1306::circle(int x0, int y0, int r, bool invert)
{
    int x;
    int y;
    int err;
    int old_err;

    x = 0;
    y = -r;
    err = 2 - 2 * r;
    do {
        pixel(x0 - x, y0 + y, invert);
        pixel(x0 - y, y0 - x, invert);
        pixel(x0 + x, y0 - y, invert);
        pixel(x0 + y, y0 + x, invert);
        if ((old_err = err) <= x) err += ++x * 2 + 1;
        if (old_err > y || err > x) err += ++y * 2 + 1;
    } while (y < 0);
}

void SSD1306::cursor(int x0, int y0, int r, bool invert)
{
    line(x0 - r, y0, x0 + r, y0, invert);
    line(x0, y0 - r, x0, y0 + r, invert);
}

void SSD1306::invert(uint8_t *buf, size_t blen)
{
    for (size_t i = 0; i < blen; i++) {
        buf[i] = ~buf[i];
    }
}

void SSD1306::flip(uint8_t *buf, size_t blen)
{
    for (size_t i = 0; i < blen; i++) {
        buf[i] = rotateByte(buf[i]);
    }
}

uint8_t SSD1306::copyBit(uint8_t src, int srcBits, uint8_t dst, int dstBits)
{
    uint8_t smask = 0x01 << srcBits;
    uint8_t dmask = 0x01 << dstBits;
    uint8_t _src = src & smask;
    uint8_t _dst;
    if (_src != 0) {
        _dst = dst | dmask;
    } else {
        _dst = dst & ~(dmask);
    }
    return _dst;
}

uint8_t SSD1306::rotateByte(uint8_t ch1)
{
    uint8_t ch2 = 0;
    for (int j = 0; j < 8; j++) {
        ch2 = (ch2 << 1) + (ch1 & 0x01);
        ch1 = ch1 >> 1;
    }
    return ch2;
}

void SSD1306::spiContrast(int contrast)
{
    if (contrast < 0x0) contrast = 0;
	if (contrast > 0xFF) contrast = 0xFF;
	writeCommand(OLED_CMD_SET_CONTRAST);       // 81
	writeCommand((uint8_t)contrast);
}

void SSD1306::i2cContrast(int contrast)
{
    uint8_t _contrast = contrast;
    if (contrast < 0x0) _contrast = 0;
    if (contrast > 0xFF) _contrast = 0xFF;

    uint8_t out_buf[3];
    int out_index = 0;
    out_buf[out_index++] = OLED_CONTROL_BYTE_CMD_STREAM; // 00
    out_buf[out_index++] = OLED_CMD_SET_CONTRAST; // 81
    out_buf[out_index++] = _contrast;

    // esp_err_t res = i2cInterface.write(out_buf, 3);
	esp_err_t res = i2c_master_transmit(mI2CDev, out_buf, out_index, -1);
    if (res != ESP_OK)
        ESP_LOGE(TAG, "Could not write to device [0x%02x]: %d (%s)", _address, res, esp_err_to_name(res));
}

void SSD1306::spiHardwareScroll(ScrollType scroll)
{
    if (scroll == SCROLL_RIGHT) {
		writeCommand(OLED_CMD_HORIZONTAL_RIGHT);	// 26
		writeCommand(0x00); // Dummy byte
		writeCommand(0x00); // Define start page address
		writeCommand(0x07); // Frame frequency
		writeCommand(0x07); // Define end page address
		writeCommand(0x00); //
		writeCommand(0xFF); //
		writeCommand(OLED_CMD_ACTIVE_SCROLL);		// 2F
	} 

	if (scroll == SCROLL_LEFT) {
		writeCommand(OLED_CMD_HORIZONTAL_LEFT);	// 27
		writeCommand(0x00); // Dummy byte
		writeCommand(0x00); // Define start page address
		writeCommand(0x07); // Frame frequency
		writeCommand(0x07); // Define end page address
		writeCommand(0x00); //
		writeCommand(0xFF); //
		writeCommand(OLED_CMD_ACTIVE_SCROLL);		// 2F
	} 

	if (scroll == SCROLL_DOWN) {
		writeCommand(OLED_CMD_CONTINUOUS_SCROLL);	// 29
		writeCommand(0x00); // Dummy byte
		writeCommand(0x00); // Define start page address
		writeCommand(0x07); // Frame frequency
		//write_command(0x01); // Define end page address
		writeCommand(0x00); // Define end page address
		writeCommand(0x3F); // Vertical scrolling offset

		writeCommand(OLED_CMD_VERTICAL);			// A3
		writeCommand(0x00);
		if (_height == 64)
			writeCommand(0x40);
		if (_height == 32)
			writeCommand(0x20);
		writeCommand(OLED_CMD_ACTIVE_SCROLL);		// 2F
	}

	if (scroll == SCROLL_UP) {
		writeCommand(OLED_CMD_CONTINUOUS_SCROLL);	// 29
		writeCommand(0x00); // Dummy byte
		writeCommand(0x00); // Define start page address
		writeCommand(0x07); // Frame frequency
		//write_command(0x01); // Define end page address
		writeCommand(0x00); // Define end page address
		writeCommand(0x01); // Vertical scrolling offset

		writeCommand(OLED_CMD_VERTICAL);			// A3
		writeCommand(0x00);
		if (_height == 64)
			writeCommand(0x40);
		if (_height == 32)
			writeCommand(0x20);
		writeCommand(OLED_CMD_ACTIVE_SCROLL);		// 2F
	}

	if (scroll == SCROLL_STOP) {
		writeCommand(OLED_CMD_DEACTIVE_SCROLL);	// 2E
	}
}

void SSD1306::i2cHardwareScroll(ScrollType scroll)
{
    uint8_t out_buf[11];
	int out_index = 0;
	out_buf[out_index++] = OLED_CONTROL_BYTE_CMD_STREAM; // 00

	if (scroll == SCROLL_RIGHT) {
		out_buf[out_index++] = OLED_CMD_HORIZONTAL_RIGHT; // 26
		out_buf[out_index++] = 0x00; // Dummy byte
		out_buf[out_index++] = 0x00; // Define start page address
		out_buf[out_index++] = 0x07; // Frame frequency
		out_buf[out_index++] = 0x07; // Define end page address
		out_buf[out_index++] = 0x00; //
		out_buf[out_index++] = 0xFF; //
		out_buf[out_index++] = OLED_CMD_ACTIVE_SCROLL; // 2F
	} 

	if (scroll == SCROLL_LEFT) {
		out_buf[out_index++] = OLED_CMD_HORIZONTAL_LEFT; // 27
		out_buf[out_index++] = 0x00; // Dummy byte
		out_buf[out_index++] = 0x00; // Define start page address
		out_buf[out_index++] = 0x07; // Frame frequency
		out_buf[out_index++] = 0x07; // Define end page address
		out_buf[out_index++] = 0x00; //
		out_buf[out_index++] = 0xFF; //
		out_buf[out_index++] = OLED_CMD_ACTIVE_SCROLL; // 2F
	} 

	if (scroll == SCROLL_DOWN) {
		out_buf[out_index++] = OLED_CMD_CONTINUOUS_SCROLL; // 29
		out_buf[out_index++] = 0x00; // Dummy byte
		out_buf[out_index++] = 0x00; // Define start page address
		out_buf[out_index++] = 0x07; // Frame frequency
		//out_buf[out_index++] = 0x01; // Define end page address
		out_buf[out_index++] = 0x00; // Define end page address
		out_buf[out_index++] = 0x3F; // Vertical scrolling offset

		out_buf[out_index++] = OLED_CMD_VERTICAL; // A3
		out_buf[out_index++] = 0x00;
		if (_height == 64)
		//out_buf[out_index++] = 0x7F;
		out_buf[out_index++] = 0x40;
		if (_height == 32)
		out_buf[out_index++] = 0x20;
		out_buf[out_index++] = OLED_CMD_ACTIVE_SCROLL; // 2F
	}

	if (scroll == SCROLL_UP) {
		out_buf[out_index++] = OLED_CMD_CONTINUOUS_SCROLL; // 29
		out_buf[out_index++] = 0x00; // Dummy byte
		out_buf[out_index++] = 0x00; // Define start page address
		out_buf[out_index++] = 0x07; // Frame frequency
		//out_buf[out_index++] = 0x01; // Define end page address
		out_buf[out_index++] = 0x00; // Define end page address
		out_buf[out_index++] = 0x01; // Vertical scrolling offset

		out_buf[out_index++] = OLED_CMD_VERTICAL; // A3
		out_buf[out_index++] = 0x00;
		if (_height == 64)
		//out_buf[out_index++] = 0x7F;
		out_buf[out_index++] = 0x40;
		if (_height == 32)
		out_buf[out_index++] = 0x20;
		out_buf[out_index++] = OLED_CMD_ACTIVE_SCROLL; // 2F
	}

	if (scroll == SCROLL_STOP) {
		out_buf[out_index++] = OLED_CMD_DEACTIVE_SCROLL; // 2E
	}

	// esp_err_t res = i2cInterface.write(out_buf, out_index);
	esp_err_t res = i2c_master_transmit(mI2CDev, out_buf, out_index, -1);
	if (res != ESP_OK)
		ESP_LOGE(TAG, "Could not write to device [0x%02x]: %d (%s)", _address, res, esp_err_to_name(res));
}

void SSD1306::spiDisplayImage(int page, int seg, uint8_t *images, int width)
{
    if (page >= _pages) return;
	if (seg >= _width) return;

	int _seg = seg + CONFIG_OFFSETX;
	uint8_t columLow = _seg & 0x0F;
	uint8_t columHigh = (_seg >> 4) & 0x0F;

	int _page = page;
	if (_flip) {
		_page = (_pages - page) - 1;
	}

	// Set Lower Column Start Address for Page Addressing Mode
	writeCommand(0x00 + columLow);
	// Set Higher Column Start Address for Page Addressing Mode
	writeCommand(0x10 + columHigh);
	// Set Page Start Address for Page Addressing Mode
	writeCommand(0xB0 | _page);

	writeData(images, width);
}

void SSD1306::i2cDisplayImage(int page, int seg, uint8_t *images, int width)
{
    if (page >= _pages) return;
	if (seg >= _width) return;

	int _seg = seg + CONFIG_OFFSETX;
	uint8_t columLow = _seg & 0x0F;
	uint8_t columHigh = (_seg >> 4) & 0x0F;

	int _page = page;
	if (_flip) {
		_page = (_pages - page) - 1;
	}

	uint8_t *out_buf = NULL;
	out_buf = (uint8_t *)malloc(width < 4 ? 4 : width + 1);
	if (out_buf == NULL) {
		ESP_LOGE(TAG, "malloc fail");
		return;
	}
	int out_index = 0;
	out_buf[out_index++] = OLED_CONTROL_BYTE_CMD_STREAM;
	// Set Lower Column Start Address for Page Addressing Mode
	out_buf[out_index++] = (0x00 + columLow);
	// Set Higher Column Start Address for Page Addressing Mode
	out_buf[out_index++] = (0x10 + columHigh);
	// Set Page Start Address for Page Addressing Mode
	out_buf[out_index++] = 0xB0 | _page;

	esp_err_t res = i2c_master_transmit(mI2CDev, out_buf, out_index, -1);
	// res = i2cInterface.write(out_buf, out_index);
	if (res != ESP_OK)
		ESP_LOGE(TAG, "Could not write to device [0x%02x]: %d (%s)", _address, res, esp_err_to_name(res));

	out_buf[0] = OLED_CONTROL_BYTE_DATA_STREAM;
	memcpy(&out_buf[1], images, width);

	// res = i2cInterface.write(out_buf, width + 1);
	res = i2c_master_transmit(mI2CDev, out_buf, width + 1, -1);
	if (res != ESP_OK)
		ESP_LOGE(TAG, "Could not write to device [0x%02x]: %d (%s)", _address, res, esp_err_to_name(res));
	free(out_buf);
}
